/* Copyright(C) 1999, Swedish Institute of Computer Science */

#include "fd.h"
#include "dvars.h"

#if MULTI_SP_AWARE
#define scalar_product_init(A1,A2) scalar_product_init(HIDDEN_ARG, A1,A2)
#endif /* MULTI_SP_AWARE */

typedef long TERM;

struct linear_data {
  void (SPCDECL *destructor)(void *);
  void (SPCDECL *daemon)(HIDDEN_PROTO
			 Argtype,void *,SP_globref,TAGGED); /* (w,handle,attr_ref,global) */
#if MULTI_SP_AWARE
  SPEnv *spenv;
#endif /* MULTI_SP_AWARE */

  SP_globref refbase;
  int op;			/* 1(#>=), 2(#>=), 3(#=), 4(#\=) */
  long stamp;			/* increases up to backtracking */
  int units;			/* maintained incrementally */
  int nonunits;			/* maintained incrementally */
  long gcdall;			/* valid during GCD rules */
  int prunings;			/* counter */
  int nvars;			/* #terms */
  int ntargets;			/* #terms that may be targets */
  long rhs;
  long bige;
  long bigf;
  TERM *heap;
  int *vheap;
  int heapsize;
  TERM *target;
  Dvar dvar;
  struct {
    long *cmin;		/* min(ai*xi) */
    long *cmax;		/* max(ai*xi) */
    long *interval;	/* cmax-cmin */
    long *coeff;		/* ai */
    long *abscoeff;		/* |ai| */
    long *gcd;			/* valid during GCD rules */
  } term;
};


#define SV(I)   (pdata->target[I])
#define COEFF(t) (pdata->term.coeff[t])
#define ABSCOEFF(t) (pdata->term.abscoeff[t])
#define GCD(t) (pdata->term.gcd[t])
#define DVAR(t) (pdata->dvar+(t))
#define RefAttr(T) (pdata->refbase + 2*(T))
#define RefVar(T) (pdata->refbase + 2*(T) + 1)
#define CMIN(t) (pdata->term.cmin[t])
#define CMAX(t) (pdata->term.cmax[t])
#define INTERVAL(t) (pdata->term.interval[t])

static void SPCDECL linear_destructor(void *pdata_v)
{
  struct linear_data *pdata = (struct linear_data *)pdata_v;
  FD_SETUP_SPENV(pdata->spenv)

  SP_free_globrefs(pdata->refbase,pdata->nvars<<1);
  SP_free(pdata);
}

#define SWAP(I,J)				\
{						\
  TERM vi = pdata->heap[I];			\
  TERM vj = pdata->heap[J];			\
  pdata->heap[I] = vj;				\
  pdata->heap[J] = vi;				\
  pdata->vheap[vi] = (J);			\
  pdata->vheap[vj] = (I);			\
}

static void 
spheapify(struct linear_data *pdata,
	  int i)
{
  TERM *heap = pdata->heap;
  long *key = pdata->term.interval;
  
  for (;;) {
    int l = (i<<1)+1;
    int topmost = i;
    if (l<pdata->heapsize && key[heap[l]] > key[heap[topmost]])
      topmost = l;
    if (l+1<pdata->heapsize && key[heap[l+1]] > key[heap[topmost]])
      topmost = l+1;
    if (topmost==i)
      break;
    SWAP(i,topmost);
    i = topmost;
  }
}

static INLINE long gcd (long c1,long c2)
{
  if (c1 > 1 && c2 > 1) {
    while (c1 != 0 && c2 != 0) {
      c1 %= c2;
      if (c1 != 0)
	c2 %= c1;
    }
    return c1+c2;
  } else {
    return 1;
  }
}

static void 
refresh_gcd(struct linear_data *pdata)
{
  int ntargets = pdata->ntargets;
  int units = pdata->units;
  int nonunits = pdata->nonunits;
  long g;
  int i;

  if (nonunits==0)
    pdata->gcdall = 1;
  else if (units>1) {
    pdata->gcdall = 1;
    for (i=ntargets-1; i>=0; i--)
      GCD(SV(i)) = 1;
  } else if (units==1) { /* GCD(i)=gcdall=1 except for one i */
    int ix=0;			/* avoid false alarm */
    pdata->gcdall = 1;
    g = -1;
    for (i=0; i<ntargets; i++) {
      TERM elt = SV(i);
      long ac = ABSCOEFF(elt);

      GCD(elt) = 1;
      if (INTERVAL(elt)==0)
	;
      else if (ac==1)
	ix = elt;
      else if (g== -1)
	g = ac;
      else
	g = gcd(g,ac);
    }
    GCD(ix) = g;
  } else {	/* for each i, compute GCD of all coefficients except i */
    int i0;
    for (i0=0; i0<ntargets && INTERVAL(SV(i0))==0; i0++)
      ;
    g = ABSCOEFF(SV(i0));
    for (i=i0+1; i<ntargets; i++) {
      TERM elt = SV(i);
      
      if (INTERVAL(elt)>0) {
	GCD(elt) = g;
	g = gcd(g,ABSCOEFF(elt));
      }
    }
    pdata->gcdall = g;
				/* now, SV(i)->gcd = gcd(a[0] ... a[i-1]) */
    for (i=ntargets-1; i>=0 && INTERVAL(SV(i))==0; i--)
      ;
    g = ABSCOEFF(SV(i));
    for (i--; i>i0; i--) {
      TERM elt = SV(i);
      
      if (INTERVAL(elt)>0) {
	GCD(elt) = gcd(g,GCD(elt));
	g = gcd(g,ABSCOEFF(elt));
      }
    }
    GCD(SV(i0)) = g;
				/* now, SV(i)->gcd = gcd({a[j] | i!=j}) */
  }
}

static void SPCDECL 
linear_daemon MAGIC (HIDDEN_PROTO
		     Argdecl,
		     void *vdata,
		     SP_globref attr_ref,
		     TAGGED global)
{
  struct linear_data *pdata = (struct linear_data *)vdata;
  TAGGED tstate;
  int ar, state_stamp;

  tstate = RefMutable(CTagToArg(global,1));
  ar = Arity(TagToHeadfunctor(tstate));
  state_stamp = GetSmall(CTagToArg(tstate,ar));
  if (pdata->stamp==state_stamp) { /* incremental */
    int j = (attr_ref - pdata->refbase)>>1;
    long c = COEFF(j);
    long ac = ABSCOEFF(j);
    Dvar dv = DVAR(j);
    long cminj;
    long cmaxj;
    TERM elt;

    dvar_init(dv, attr_ref, attr_ref+1);
    if (c>0) {
      cminj = c*dvar_min_l(dv);
      cmaxj = c*dvar_max_l(dv);
    } else {
      cmaxj = c*dvar_min_l(dv);
      cminj = c*dvar_max_l(dv);
    }
    if (cminj==CMIN(j) && cmaxj==CMAX(j))
      return;			/* self-invocation */
    (void)fd_daemon_copy_state(w,&global);
    pdata->stamp++;
    pdata->bigf += CMIN(j);
    pdata->bige -= CMAX(j);
    CMIN(j) = cminj;
    CMAX(j) = cmaxj;
    INTERVAL(j) = cmaxj - cminj;
    pdata->bigf -= cminj;
    pdata->bige += cmaxj;
    if (dvar_is_integer(dv)) {
      if (ac==1)
	pdata->units--;
      else
	pdata->nonunits--;
    }
    if (pdata->units+pdata->nonunits==0)
      goto push;
    switch (pdata->op) {
    case 1:
      spheapify(pdata,pdata->vheap[j]);
      elt = pdata->heap[0];
      if (pdata->bigf < INTERVAL(elt))
	goto push;
      return;
    case 2:
      spheapify(pdata,pdata->vheap[j]);
      elt = pdata->heap[0];
      if (pdata->bige < INTERVAL(elt))
	goto push;
      return;
    case 3:
      spheapify(pdata,pdata->vheap[j]);
      elt = pdata->heap[0];
      if (pdata->bigf < INTERVAL(elt) ||
	  pdata->bige < INTERVAL(elt))
	goto push;
      return;
    case 4:
      if (pdata->units+pdata->nonunits <= 1)
	goto push;
      return;
    }
  }
 push:
  fd_enqueue_global(w, global, 0x5/* MINMAX, append*/);   
}

static SP_BOOL
scalar_product_init MAGIC (HIDDEN_PROTO
			   struct linear_data *pdata,
			   SP_BOOL incremental)
{
  int op = pdata->op;
  int i;
  HIDDEN_ARG_USE;
  /* Phase 1: compute:
     bigf = rhs - sum{min(a_i x_i)}
     bige = sum{max(a_i x_i)} - rhs

     I_i = max(a_i x_i) - min(a_i x_i)
  */
  if (incremental) {
    for (i=0; i<pdata->ntargets; i++) {
      int j = SV(i);
      Dvar dv = DVAR(j);
      dvar_init(dv, RefAttr(j), RefVar(j));
      if (dvar_is_integer(dv))
	pdata->rhs -= CMIN(j);
#if DBG
      {
	long c = COEFF(j);
	if (c>0) {
	  long cminj = c*dvar_min_l(dv);
	  long cmaxj = c*dvar_max_l(dv);
	  SP_ASSERT((cminj==CMIN(j) && cmaxj==CMAX(j)));
	} else {
	  long cmaxj = c*dvar_min_l(dv);
	  long cminj = c*dvar_max_l(dv);
	  SP_ASSERT((cminj==CMIN(j) && cmaxj==CMAX(j)));
	}
      }
#endif
    }
  } else {
    pdata->bigf = pdata->rhs;
    pdata->bige = - pdata->bigf;
    pdata->units = 0;
    pdata->nonunits = 0;
    for (i=0; i<pdata->ntargets; i++) {
      int j = SV(i);
      long c = COEFF(j);
      long ac = ABSCOEFF(j);
      Dvar dv = DVAR(j);

      dvar_init(dv, RefAttr(j), RefVar(j));
      if (c>0) {
	CMIN(j) = c*dvar_min_l(dv);
	CMAX(j) = c*dvar_max_l(dv);
      } else {
	CMAX(j) = c*dvar_min_l(dv);
	CMIN(j) = c*dvar_max_l(dv);
      }
      INTERVAL(j) = CMAX(j) - CMIN(j);
      if (op<4) {
	pdata->heap[i] = j;
	pdata->vheap[j] = i;
      }
      pdata->bigf -= CMIN(j);
      pdata->bige += CMAX(j);
      if (dvar_is_integer(dv)) {
	pdata->rhs -= CMIN(j);
      } else if (ac==1) {
	pdata->units++;
      } else {
	pdata->nonunits++;
      }
    }
    pdata->heapsize = i;
    if (op<4)
      for (i=(pdata->heapsize-2)>>1; i>=0; i--)
	spheapify(pdata,i);
  }
  return TRUE;
}

static SP_BOOL
scalar_product_le(struct linear_data *pdata, TERM elt)
{
  long c = COEFF(elt);
  Dvar dv = DVAR(elt);
  long decr;
  long cmax0 = CMAX(elt);
  long cmax;

  if (pdata->bigf<0)
    return FALSE;
  if (c>0) {
    long ub = pdata->bigf/c + dvar_min_l(dv);
    if (dvar_fix_max_l(dv,ub)<0)
      return FALSE;
    cmax = c*dvar_max_l(dv);
  } else {
    long lb = -(pdata->bigf/(-c)) + dvar_max_l(dv);
    if (dvar_fix_min_l(dv,lb)<0)
      return FALSE;
    cmax = c*dvar_min_l(dv);
  }
  CMAX(elt) = cmax;
  decr = cmax0-cmax;
  INTERVAL(elt) -= decr;
  spheapify(pdata,0);
  pdata->bige -= decr;
  if (dvar_is_integer(dv)) {
    pdata->rhs -= cmax;
    if (ABSCOEFF(elt)==1)
      pdata->units--;
    else
      pdata->nonunits--;
  }
  return TRUE;
}

static SP_BOOL
scalar_product_ge(struct linear_data *pdata, TERM elt)
{
  long c = COEFF(elt);
  Dvar dv = DVAR(elt);
  long decr;
  long cmin0 = CMIN(elt);
  long cmin;

  if (pdata->bige<0)
    return FALSE;
  if (c>0) {
    long lb = -(pdata->bige/c) + dvar_max_l(dv);
    if (dvar_fix_min_l(dv,lb)<0)
      return FALSE;
    cmin = c*dvar_min_l(dv);
  } else {
    long ub = pdata->bige/(-c) + dvar_min_l(dv);
    if (dvar_fix_max_l(dv,ub)<0)
      return FALSE;
    cmin = c*dvar_max_l(dv);
  }
  CMIN(elt) = cmin;
  decr = cmin-cmin0;
  INTERVAL(elt) -= decr;
  spheapify(pdata,0);
  pdata->bigf -= decr;
  if (dvar_is_integer(dv)) {
    pdata->rhs -= cmin;
    if (ABSCOEFF(elt)==1)
      pdata->units--;
    else
      pdata->nonunits--;
  }
  return TRUE;
}

/* Preconditions:
   0<coeff<mod, 0=<rhs<mod, 0<mod

   Solve min X such that coeff*X = rhs (modulo mod)
*/
static long solve_gcd(long coeff, long rhs, long mod)
{
  if (rhs==0)
    return 0;
  else {
    long rhs1 = rhs%coeff;
    if (rhs1==0)
      return rhs/coeff;
    else
      return (rhs + mod*solve_gcd(mod%coeff, coeff-rhs1, coeff)) / coeff;
  }
}

/* Preconditions: 
   0<mod, gcdall = gcd(coeff,mod)

   Adjusts minx up and maxx down s.t.
   Returns smallest s.t. coeff*minx = coeff*maxx = rhs (modulo mod)
*/
static void 
adjust_bounds_gcd(long coeff, long rhs,
		  long mod, long gcdall,
		  long *minx, long *maxx)
{
  long minx0 = *minx, maxx0 = *maxx;
  long q = mod/gcdall;
  long r, x, s;
  long rhslocal;

  if (coeff>0) {
    rhslocal = rhs;
  } else {
    rhslocal = -rhs;
    coeff = -coeff;
  }
  coeff %= mod;
  rhslocal %= mod;
  if (rhslocal<0)		/* ensure mod, not rem */
    rhslocal += mod;
  s = solve_gcd(coeff, rhslocal, mod);
  r = minx0 % q;
  if (r<0)
    r += q;
  x = minx0 - r + s;
  if (x<minx0)
    x += q;
  *minx = x;
  r = maxx0 % q;
  if (r<0)
    r += q;
  x = maxx0 - r + s;
  if (x>maxx0)
    x -= q;
  *maxx = x;
}

SP_BOOL
fd_linear_filter_fast MAGIC(HIDDEN_PROTO Argdecl,SP_term_ref State)
{
  TAGGED tvec, telt, handle, t1;
  SP_BOOL committed; /* TRUE if state can't be backtracked over */
  int nvars, i;
  long state_stamp;
  int ent = -1, total_size;
  int nonground = 0;
  char *ptr;
  struct linear_data *pdata;

  w->numstack_end = NULL;
/*    X(0) = RefTerm(State0); */
  dvar_export_start(w);
  RefTerm(State) = fd_unify_output_state(w,&handle,&state_stamp,&committed);

  if (!IsVar(handle)) {		/* got [Flag | '$free'(Ptr)] */
    pdata = Pdata(struct linear_data,handle);
    nvars = pdata->nvars;
  } else {			/* build persistent state */
    DerefArg(tvec,X(0),1);	/* get CX0 */
    nvars = fd_list_length(tvec);	/* count terms */
    total_size = 
      nvars*sizeof(struct dvar) +
      6*nvars*sizeof(long) +
      2*nvars*sizeof(TERM) +
      nvars*sizeof(int);
  
    pdata = Palloc(struct linear_data, total_size, handle);
    ptr = (char *)(pdata+1);
    pdata->term.cmin = (long *)ptr;
    ptr += nvars*sizeof(long);
    pdata->term.cmax = (long *)ptr;
    ptr += nvars*sizeof(long);
    pdata->term.interval = (long *)ptr;
    ptr += nvars*sizeof(long);
    pdata->dvar = (Dvar)ptr;
    ptr += nvars*sizeof(struct dvar);
    pdata->term.coeff = (long *)ptr;
    ptr += nvars*sizeof(long);
    pdata->term.abscoeff = (long *)ptr;
    ptr += nvars*sizeof(long);
    pdata->term.gcd = (long *)ptr;
    ptr += nvars*sizeof(long);
    pdata->target = (TERM *)ptr;
    ptr += nvars*sizeof(TERM);
    pdata->heap = (TERM *)ptr;
    ptr += nvars*sizeof(TERM);
    pdata->vheap = (int *)ptr;
    ptr += nvars*sizeof(int);
    SP_ASSERT(ptr == (char *)(pdata+1)+total_size);
    FD_STORE_SPENV(pdata->spenv);
    pdata->refbase = SP_alloc_globrefs(nvars<<1);
    pdata->stamp = state_stamp-1; /* don't trust initially */
    DerefArg(telt,X(0),2);
    pdata->op = GetSmall(telt) & 0x7;
    pdata->destructor = linear_destructor;
    pdata->daemon = linear_daemon;
    pdata->nvars = nvars;
    pdata->ntargets = nvars;
    DerefArg(tvec,X(0),1);	/* get CX0 */
    DerefArg(telt,X(0),3);	/* get RHS */
    pdata->rhs = GetSmall(telt);
				/* xfer all info to the struct linear_terms */
    for (i=0; i<nvars; i++) {
      TERM elt = i;
      long c;
      
      DerefCar(telt,tvec);
      DerefCdr(tvec,tvec);
      DerefArg(t1,telt,1);
      COEFF(elt) = c = GetSmall(t1);
      ABSCOEFF(elt) = (c>=0 ? c : -c);
      fd_get_var_and_attr(telt+WD(1),RefAttr(elt));
      SV(i) = elt;
    }
    CTagToArg(X(0),1) = atom_nil; /* [MC] 3.12: free for GC */
    for (i=0; i<pdata->ntargets; i++) {
      int j = SV(i);
      Dvar dv = DVAR(j);
      TAGGED functor;
      dvar_init(dv, RefAttr(j), RefVar(j));
      switch (pdata->op) {
      case 4:
	functor = fd.functor_val;
	break;
      default:
	functor = fd.functor_minmax;
	break;
      }
      dvar_attach_daemon(w, dv, pdata, X(1), functor);
    }
  }
  
				/* RESUME HERE */

  if (pdata->op==4 || /* #\= case: CMIN/CMAX are not known to be up to date */
      state_stamp != pdata->stamp || /* after backtracking: CMIN/CMAX are not known to be up to date */
      !scalar_product_init(pdata,TRUE)) {
    DerefArg(telt,X(0),3);
    pdata->rhs = GetSmall(telt);
    DerefArg(telt,X(0),4);
    pdata->ntargets = pdata->nvars-GetSmall(telt);
    scalar_product_init(pdata,FALSE);
  }
  pdata->stamp = state_stamp+1;
    
  /* (dis)entailment tests based on GCD */
  switch (pdata->op) {
  case 3:
    refresh_gcd(pdata);	/* compute gcdall, GCD() */
    if (pdata->gcdall>1 && pdata->rhs % pdata->gcdall!=0)
      return -1;
    break;
  case 4:
    refresh_gcd(pdata);	/* compute gcdall, GCD() */
    if (pdata->gcdall>1 && pdata->rhs % pdata->gcdall!=0) {
      return 1;
    }
    break;
  }
 loop:
  switch (pdata->op) {
  case 1:			/* #=< */
    if (pdata->heapsize>0) {
      TERM elt = pdata->heap[0];
      if (pdata->bigf < INTERVAL(elt)) {
	if (!scalar_product_le(pdata,elt)) {
	  return -1;
	} else {
	  goto loop;
	}
      }
    }
    break;
  case 2:			/* #>= */
    if (pdata->heapsize>0) {
      TERM elt = pdata->heap[0];
      if (pdata->bige < INTERVAL(elt)) {
	if (!scalar_product_ge(pdata,elt)) {
	  return -1;
	} else {
	  goto loop;
	}
      }
    }
    break;
  case 3:			/* #= */
    if (pdata->heapsize>0) {
      TERM elt = pdata->heap[0]; 
      if (pdata->bigf < INTERVAL(elt)) {
	if (!scalar_product_le(pdata,elt))
	  return -1;
	goto loop;
      }
      if (pdata->bige < INTERVAL(elt)) {
	if (!scalar_product_ge(pdata,elt))
	  return -1;
	goto loop;
      }
      refresh_gcd(pdata);	/* compute gcdall, GCD() */
      if (pdata->rhs % pdata->gcdall != 0)
	return -1;
      if (pdata->units<=1)
	for (i=0; i<pdata->ntargets; i++) {
	  TERM elt = SV(i);
	  Dvar dv = DVAR(elt);
	  long imin = dvar_min_l(dv);
	  long imax = dvar_max_l(dv);

	  if (imin<imax && GCD(elt)>pdata->gcdall) {
	    /* Ensure that:

	       Ai * min(Xi) - RHS = Ai * max(Xi) - RHS = 0 (modulo G)

	       where G is the gcd of all coefficients excepts Ai.
	       Fails if an empty domain is produced.
	    */
	    int rc;

	    adjust_bounds_gcd(COEFF(elt),
			      pdata->rhs,
			      GCD(elt),
			      pdata->gcdall,
			      &imin, &imax);
	    rc = dvar_fix_interval_l(dv, imin, imax);
	    if (rc<0)
	      return -1;
	    if (rc>0) {
	      long c = COEFF(elt);
	      long cmin0 = CMIN(elt);
	      long cmax0 = CMAX(elt);
	      long decr;

	      if (c>0) {
		CMIN(elt) = c*dvar_min_l(dv);
		CMAX(elt) = c*dvar_max_l(dv);
	      } else {
		CMAX(elt) = c*dvar_min_l(dv);
		CMIN(elt) = c*dvar_max_l(dv);
	      }
	      INTERVAL(elt) = CMAX(elt) - CMIN(elt);
	      decr = cmax0 - CMAX(elt);
	      pdata->bige -= decr;
	      decr = CMIN(elt) - cmin0;
	      pdata->bigf -= decr;
	      spheapify(pdata,pdata->vheap[elt]);
	      if (dvar_is_integer(dv)) {
		pdata->rhs -= CMIN(elt);
		if (ABSCOEFF(elt)==1)
		  pdata->units--;
		else
		  pdata->nonunits--;
	      }
	      goto loop;
	    }
	  }
	}
    }
    break;
  case 4:			/* #\= */
    nonground = pdata->units+pdata->nonunits;
    if (nonground==0 && pdata->rhs==0)
      return -1;
    else if (nonground==1) {
      for (i=0; INTERVAL(SV(i))==0; i++)
	;
      {
	TERM elt = SV(i);
	Dvar dv = DVAR(elt);

	if (pdata->rhs % ABSCOEFF(elt)==0) { /* RHS a multiple of coefficient */
	  dvar_prune_value_l(dv,pdata->rhs/COEFF(elt),w);
	}
      }
    }
    break;
  }
  switch (pdata->op) {
  case 1:
    ent = (pdata->bigf < 0 ? -1 : pdata->bige <= 0);
    break;
  case 2:
    ent = (pdata->bige < 0 ? -1 : pdata->bigf <= 0);
    break;
  case 3:
    ent = (pdata->bigf < 0 ? -1 :
	   pdata->bige < 0 ? -1 : 
	   pdata->bigf > 0 ? 0 :
	   pdata->bige > 0 ? 0 : 1);
    break;
  case 4:
    ent = (nonground<=1);
    break;
  }

  if (ent>=0) {
    int inf = 0;
    int sup = pdata->ntargets-1;
    TERM held = SV(sup); /* sup is the hole */
    TERM current = SV(inf);
    
    while (inf<=sup) {
      Dvar dv = DVAR(current);
      
      dvar_pruning_done(w,dv);
      dvar_export(w,dv);
      if (!dvar_is_integer(dv)) {
	SV(inf) = current;
	inf++;
	current = (inf>=sup ? held : SV(inf));
      } else {
	SV(sup) = current;
	sup--;
	current = (inf>=sup ? held : SV(sup));
      }
    }
    pdata->ntargets = inf;
  }
  if (ent==0 && pdata->op==3 && pdata->ntargets==2 && pdata->rhs==0) {
    TERM elt0 = SV(0);
    TERM elt1 = SV(1);

    if (COEFF(elt0) == -COEFF(elt1)) {
      dvar_export_equal(w,DVAR(elt0), DVAR(elt1));
      ent = 1;
    }
  }
  CTagToArg(X(0),3) = MakeSmall(pdata->rhs); /* TODO: SAFE? */
  CTagToArg(X(0),4) = MakeSmall(pdata->nvars-pdata->ntargets);
  return ent;
}
